# ros_bridge/ros_node.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import threading
import json

# 导入全局状态和工具
import config.config as config
from logger.logger import ros_logger
from ws.ws import socketio  # 用于 WebSocket 通信的 SocketIO 实例

class RosBridgeNode(Node):
    """ROS2 节点，将 ROS 话题数据桥接到 WebSocket 并发布机器人状态。"""
    def __init__(self, robot_count=1):
        super().__init__('ros_bridge_node')
        self.robot_count = robot_count
        ros_logger.info(f"ROS Bridge Node 已初始化，机器人数量：{robot_count}")

        # 订阅 NS-3 仿真话题（假设消息类型为 std_msgs/String 且内容为 JSON 格式）
        self.routing_table = None
        self.rss_matrix = None
        self.throughput_matrix = None

        self.create_subscription(String, '/routing_table', self.routing_table_callback, 10)
        self.create_subscription(String, '/rss_matrix', self.rss_matrix_callback, 10)
        self.create_subscription(String, '/throughput_matrix', self.throughput_matrix_callback, 10)

        # 为每个机器人创建当前状态话题的发布器
        self.robot_publishers = []
        for i in range(1, robot_count + 1):
            topic_name = f'/robot_{i}/current_state'
            publisher = self.create_publisher(String, topic_name, 10)
            self.robot_publishers.append(publisher)
            ros_logger.info(f"已为 {topic_name} 创建发布器")

        # 定时器：每秒发布一次机器人状态
        self.create_timer(1.0, self.publish_robot_states)

    def routing_table_callback(self, msg):
        """处理来自 NS-3 的 routing_table 更新消息。"""
        try:
            data = json.loads(msg.data)
        except Exception:
            data = msg.data  # 如果不是 JSON，则直接使用原始字符串
        self.routing_table = data
        ros_logger.info("收到 routing_table 更新。")
        self.broadcast_status_update()

    def rss_matrix_callback(self, msg):
        """处理来自 NS-3 的 rss_matrix 更新消息。"""
        try:
            data = json.loads(msg.data)
        except Exception:
            data = msg.data
        self.rss_matrix = data
        ros_logger.info("收到 rss_matrix 更新。")
        self.broadcast_status_update()

    def throughput_matrix_callback(self, msg):
        """处理来自 NS-3 的 throughput_matrix 更新消息。"""
        try:
            data = json.loads(msg.data)
        except Exception:
            data = msg.data
        self.throughput_matrix = data
        ros_logger.info("收到 throughput_matrix 更新。")
        self.broadcast_status_update()

    def broadcast_status_update(self):
        """将最新的仿真指标数据推送给所有 WebSocket 客户端。"""
        status = {
            "routing_table": self.routing_table,
            "rss_matrix": self.rss_matrix,
            "throughput_matrix": self.throughput_matrix
        }
        socketio.emit('network_status', status, broadcast=True)
        ros_logger.info(f"已向 WebSocket 客户端广播状态更新：{json.dumps(status)}")

    def publish_robot_states(self):
        """周期性发布每个机器人的当前状态。"""
        running = config.settings.get("simulation_running", False)
        state_text = "ACTIVE" if running else "INACTIVE"
        for i, publisher in enumerate(self.robot_publishers, start=1):
            msg = String()
            msg.data = f"Robot {i} state: {state_text}"
            publisher.publish(msg)
        ros_logger.info(f"已发布机器人状态（{state_text}）。")

# 辅助函数：在后台线程中运行 ROS2 节点
_ros_thread = None

def start_ros_thread(robot_count=1):
    """启动 ROS2 线程（如果尚未启动）。"""
    global _ros_thread
    if _ros_thread and _ros_thread.is_alive():
        return  # ROS 线程已在运行
    def ros_spin():
        rclpy.init(args=None)
        node = RosBridgeNode(robot_count=robot_count)
        try:
            rclpy.spin(node)  # 保持节点运行以处理回调
        except KeyboardInterrupt:
            pass
        # 退出时清理
        node.destroy_node()
        rclpy.shutdown()
    _ros_thread = threading.Thread(target=ros_spin, daemon=True)
    _ros_thread.start()
    ros_logger.info("ROS2 后台线程已启动。")
